#include "driver/dev_imu.h"

IMU::IMU(/* args */)
{
    last_update_time = 0;
    last_diff_time = 0;

    ax = 0; ay = 0; az = 0;
    gx = 0; gy = 0; gz = 0;

    flag = 0;
    imu_encoder_diff  = 0;
    imu_encoder_state = 0;
}

IMU::~IMU()
{
}

void IMU::init()
{
    Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
    Wire.setClock(400000);
    while (!imu.testConnection());
    imu.initialize();
}

void IMU::update(int interval)
{
    if (millis() < last_update_time + interval)
    {
        return;
    }
    last_update_time = millis();

    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    Serial.print(gx);
    Serial.print(" ");
    Serial.print(gy);
    Serial.print(" ");
    Serial.print(gz);
    Serial.println(" ");

    Serial.print(ax);
    Serial.print(" ");
    Serial.print(ay);
    Serial.print(" ");
    Serial.print(az);
    Serial.println(" ");

    /* encoder diff */
    if (ay > 3000)
    {
        if (flag == 0)
        {
            imu_encoder_diff++;
            last_diff_time = millis();
        }
        else
        {
            if (millis() > last_diff_time + 1000)
            {
                imu_encoder_diff++;
                last_diff_time = millis();
            }
        }
        flag = 1;
    }
    else if (ay < -3000)
    {
        if (flag == 0)
        {
            imu_encoder_diff--;
            last_diff_time = millis();
        }
        else
        {
            if (millis() > last_diff_time + 1000)
            {
                imu_encoder_diff--;
                last_diff_time = millis();
            }
        }
        flag = 1;
    }
    else
    {
        flag = 0;
    }

    /* encoder state */
    if (ax > 10000)
    {
        imu_encoder_state = 1;
    }
    else
    {
        imu_encoder_state = 0;
    }

    // Serial.print(imu_encoder_diff);
    // Serial.print(" ");
    // Serial.print(imu_encoder_state);
    // Serial.println(" ");
}

int IMU::get_imu_encoder_diff()
{
    int diff = imu_encoder_diff;

    /* 每次被获取一次,就要少一次 */
    if (imu_encoder_diff > 0)
    {
        imu_encoder_diff--;
    }
    else if (imu_encoder_diff < 0)
    {
        imu_encoder_diff++;
    }

    return diff;
}

int IMU::get_imu_encoder_state()
{
    return imu_encoder_state;
}

IMU mpu;
